#pragma once

#include "vector.h"



template <int TEMPL_MAT_DIM, class ARG_TYPE>
class mdsMatrix 
{
	ARG_TYPE m_arrVals[TEMPL_MAT_DIM][TEMPL_MAT_DIM];
public:
	mdsMatrix()
	{
		for(int i = 0; i < TEMPL_MAT_DIM; ++i)
		{
			for(int j = 0; j < TEMPL_MAT_DIM; ++j)
			{
				m_arrVals[i][j] = (ARG_TYPE)0.0;
			}
			m_arrVals[i][i] = (ARG_TYPE)1.0;
		}
	}

	mdsMatrix(bool bInit)
	{
		if(bInit)
		{
			for(int i = 0; i < TEMPL_MAT_DIM; ++i)
			{
				for(int j = 0; j < TEMPL_MAT_DIM; ++j)
				{
					m_arrVals[i][j] = (ARG_TYPE)0.0;
				}
				m_arrVals[i][i] = (ARG_TYPE)1.0;
			}
		}
	}



	mdsMatrix(const mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> & matIn)
	{
		for(int i = 0; i < TEMPL_MAT_DIM; ++i)
		{
			for(int j = 0; j < TEMPL_MAT_DIM; ++j)
			{
				m_arrVals[i][j] = matIn.m_arrVals[i][j];
			}
			
		}
	}

	mdsMatrix(int iAxisRotFrom, int iAxisRotTo, ARG_TYPE fRotInRads) // assumes pre-multiply convention
	{
		for(int i = 0; i < TEMPL_MAT_DIM; ++i)
		{
			for(int j = 0; j < TEMPL_MAT_DIM; ++j)
			{
				m_arrVals[i][j] = (ARG_TYPE)0.0;
			}
			m_arrVals[i][i] = (ARG_TYPE)1.0;
		}
		ARG_TYPE fSin = (ARG_TYPE)sin((double)fRotInRads);
		ARG_TYPE fCos = (ARG_TYPE)cos((double)fRotInRads);
		m_arrVals[iAxisRotFrom][iAxisRotFrom] = fCos;
		m_arrVals[iAxisRotTo][iAxisRotTo] = fCos;
		m_arrVals[iAxisRotFrom][iAxisRotTo] = -fSin;
		m_arrVals[iAxisRotTo][iAxisRotFrom] = fSin;
	}

	

	~mdsMatrix()
	{
	}

	const mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> & operator=(const mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> & matIn)
	{
		for(int i = 0; i < TEMPL_MAT_DIM; ++i)
		{
			for(int j = 0; j < TEMPL_MAT_DIM; ++j)
			{
				m_arrVals[i][j] = matIn.m_arrVals[i][j];
			}
			
		}
		return *this;
	}

	mdsVector<TEMPL_MAT_DIM, ARG_TYPE> GetRow(int nRow) const
	{
		mdsVector<TEMPL_MAT_DIM, ARG_TYPE> vecResult;
		for(int i = 0; i < TEMPL_MAT_DIM; ++i)
		{
			vecResult[i] = m_arrVals[nRow][i];
		}
		return vecResult;
	}

	mdsVector<TEMPL_MAT_DIM, ARG_TYPE> GetColumn(int nCol) const
	{
		mdsVector<TEMPL_MAT_DIM, ARG_TYPE> vecResult;
		for(int i = 0; i < TEMPL_MAT_DIM; ++i)
		{
			vecResult[i] = m_arrVals[i][nCol];
		}
		return vecResult;
	}

	mdsVector<TEMPL_MAT_DIM, ARG_TYPE> preMultVec(const mdsVector<TEMPL_MAT_DIM, ARG_TYPE> & vecIn) const
	{
		mdsVector<TEMPL_MAT_DIM, ARG_TYPE> vecResult;
		for(int i = 0; i < TEMPL_MAT_DIM; ++i)
		{
			vecResult[i] = m_arrVals[0][i]*vecIn[0];
			for(int j = 1; j < TEMPL_MAT_DIM; ++j)
			{
				vecResult[i] += m_arrVals[j][i]*vecIn[j];
			}
		}
		return vecResult;
	}

	mdsVector<TEMPL_MAT_DIM, ARG_TYPE> postMultVec(const mdsVector<TEMPL_MAT_DIM, ARG_TYPE> & vecIn) const
	{
		mdsVector<TEMPL_MAT_DIM, ARG_TYPE> vecResult;
		for(int i = 0; i < TEMPL_MAT_DIM; ++i)
		{
			vecResult[i] = m_arrVals[i][0]*vecIn[0];
			for(int j = 1; j < TEMPL_MAT_DIM; ++j)
			{
				vecResult[i] += m_arrVals[i][j]*vecIn[j];
			}
		}
		return vecResult;
	}

	

	mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> GetTranspose() const
	{
		mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> matResult;
		for(int i = 0; i < TEMPL_MAT_DIM; ++i)
		{
			for(int j = 0; j < TEMPL_MAT_DIM; ++j)
			{
				matResult.m_arrVals[i][j] = m_arrVals[j][i];
			}
		}
	}

	void TransposeMe()
	{
		for(int i = 0; i < TEMPL_MAT_DIM; ++i)
		{
			for(int j = i+1; j < TEMPL_MAT_DIM; ++j)
			{
				ARG_TYPE tmp = m_arrVals[i][j];
				m_arrVals[i][j] = m_arrVals[j][i];
				m_arrVals[j][i] = tmp;
			}
		}
	}

	mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> multMat(const mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> & matIn) const // returns this * matIn (NOT matIn*this)
	{
		mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> matResult(false);
		for(int i = 0; i < TEMPL_MAT_DIM; ++i)
		{
			for(int j = 0; j < TEMPL_MAT_DIM; ++j)
			{
				matResult.m_arrVals[i][j] = m_arrVals[i][0] * matIn.m_arrVals[0][j];
				for(int k = 1; k < TEMPL_MAT_DIM; ++k)
				{
					matResult.m_arrVals[i][j] += m_arrVals[i][k] * matIn.m_arrVals[k][j];
				}
			}
		}
		return matResult;
	}

	mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> multMatByTranspose(const mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> & matIn) const // returns this * transpose(matIn)
	{
		mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> matResult(false);
		for(int i = 0; i < TEMPL_MAT_DIM; ++i)
		{
			for(int j = 0; j < TEMPL_MAT_DIM; ++j)
			{
				matResult.m_arrVals[i][j] = m_arrVals[i][0] * matIn.m_arrVals[j][0];
				for(int k = 1; k < TEMPL_MAT_DIM; ++k)
				{
					matResult.m_arrVals[i][j] += m_arrVals[i][k] * matIn.m_arrVals[j][k];
				}
			}
		}
		return matResult;
	}

	mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> multMatByTransposeAndTranspose(const mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> & matIn) const // returns transpose(this * transpose(matIn))
	{
		mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> matResult(false);
		for(int i = 0; i < TEMPL_MAT_DIM; ++i)
		{
			for(int j = 0; j < TEMPL_MAT_DIM; ++j)
			{
				matResult.m_arrVals[j][i] = m_arrVals[i][0] * matIn.m_arrVals[j][0];
				for(int k = 1; k < TEMPL_MAT_DIM; ++k)
				{
					matResult.m_arrVals[j][i] += m_arrVals[i][k] * matIn.m_arrVals[j][k];
				}
			}
		}
		return matResult;
	}

	

	inline const ARG_TYPE * operator[](int nRow) const
	{
		// return &(m_arrVals[nRow][0]);
		return m_arrVals[nRow];
	}

	inline ARG_TYPE * operator[](int nRow) 
	{
		// return &(m_arrVals[nRow][0]);
		return m_arrVals[nRow];
	}

	

	const mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> & scaleUniform(ARG_TYPE fScale) 
	{
		for(int i = 0; i < TEMPL_MAT_DIM; ++i)
		{
			for(int j = 0; j < TEMPL_MAT_DIM; ++j)
			{
				m_arrVals[i][j] *= fScale;
			}
		}
		return *this;

	}

	const mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> & swapRows(int iRow1, int iRow2)
	{
		for(int iCol = 0; iCol < TEMPL_MAT_DIM; ++iCol)
		{
			ARG_TYPE fTmp = m_arrVals[iRow1][iCol];
			m_arrVals[iRow1][iCol] = m_arrVals[iRow2][iCol];
			m_arrVals[iRow2][iCol] = fTmp;
		}
		return *this;
	}

	const mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> & swapCols(int iCol1, int iCol2)
	{
		for(int iRow = 0; iRow < TEMPL_MAT_DIM; ++iRow)
		{
			ARG_TYPE fTmp = m_arrVals[iRow][iCol1];
			m_arrVals[iRow][iCol1] = m_arrVals[iRow][iCol2];
			m_arrVals[iRow][iCol2] = fTmp;
		}
		return *this;
	}

	const mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> & subtractRow(int iRowSubtract, int iRowSubtractFrom, ARG_TYPE fWeight)
	{
		for(int iCol = 0; iCol < TEMPL_MAT_DIM; ++iCol)
		{
			m_arrVals[iRowSubtractFrom][iCol] -=
				fWeight*m_arrVals[iRowSubtract][iCol];
		}
		return *this;
	}

	const mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> & scaleRow(int iRow, ARG_TYPE fScale)
	{
		for(int iCol = 0; iCol < TEMPL_MAT_DIM; ++iCol)
		{
			m_arrVals[iRow][iCol] *= fScale;
		}
		return *this;
	}

	mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> getInverse(bool & bSingular) const
	{
		bSingular = false;
		mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> matResult;

		mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> matMakeId = *this;
		// do gauss-jordan

		int iRow;

		for(iRow = 0; iRow < TEMPL_MAT_DIM; ++iRow)
		{
			int iPivot = iRow;
			for(int iCandidatePivot = iPivot; iCandidatePivot < TEMPL_MAT_DIM; ++iCandidatePivot)
			{
				if(matMakeId[iCandidatePivot][iRow] > matMakeId[iPivot][iRow])
				{
					iPivot = iCandidatePivot;
				}
			}
			if(iPivot != iRow)
			{
				matResult.swapRows(iRow, iPivot);
				matMakeId.swapRows(iRow, iPivot);
			}

			if(fabs(matMakeId[iRow][iRow]) < GenericMath::eps)
			{
				bSingular = true;
				mdsMatrix<TEMPL_MAT_DIM, ARG_TYPE> matId;
				return matId;
			}

			ARG_TYPE fScale = static_cast<ARG_TYPE>(1.0/matMakeId[iRow][iRow]);

			matMakeId.scaleRow(iRow, fScale);
			matResult.scaleRow(iRow, fScale);

			for(int iRowUTriPass = iRow+1; iRowUTriPass < TEMPL_MAT_DIM; ++iRowUTriPass)
			{
				matResult.subtractRow(iRow, iRowUTriPass, matMakeId[iRowUTriPass][iRow]);
				matMakeId.subtractRow(iRow, iRowUTriPass, matMakeId[iRowUTriPass][iRow]);
			}

			for(int iRowDiagPass = iRow-1; iRowDiagPass >= 0; --iRowDiagPass)
			{
				matResult.subtractRow(iRow, iRowDiagPass, matMakeId[iRowDiagPass][iRow]);
				matMakeId.subtractRow(iRow, iRowDiagPass, matMakeId[iRowDiagPass][iRow]);
			}
			
		}


		return matResult;
	}


};



namespace matrixGlobals
{
	inline mdsMatrix<3, double> GetAxisRot(int nAxis, double lfTheta)
	{
		mdsMatrix<3, double> matResult((nAxis+1)%3, (nAxis+2)%3, lfTheta);
		return matResult;
	}

	mdsMatrix<3, double> GetRotAboutVector(const mdsVector<3, double> &, double lfTheta);

	mdsMatrix<4, double> RotMatToProjectiveXform(const mdsMatrix<3, double> &);

	mdsMatrix<3, double> GetMatRotComponent(const mdsMatrix<4, double> &);

	mdsVector<4, double> Vec3ProjectUp(const mdsVector<3, double> &);

	mdsVector<3, double> Vec4ProjectDown(const mdsVector<4, double> &);

	template<class ARG_TYPE> double GetDet(const ARG_TYPE * glMat, int dim);

	template<class ARG_TYPE> void GetCofactorMat(const ARG_TYPE * glMatIn, ARG_TYPE * glMatOut, int dim);

	template<class ARG_TYPE> void RemoveRowColumn(const ARG_TYPE * glMatIn, ARG_TYPE * glMatOut, int dim, int nRow, int nCol);

	template<class ARG_TYPE> bool InvertMat(const ARG_TYPE * glMatIn, ARG_TYPE * glMatOut, int dim);

	template<class ARG_TYPE> void m4ToGlMat(const mdsMatrix<4, double> &, ARG_TYPE * glMatOut);

	template<class ARG_TYPE> void glMatToM4(const ARG_TYPE * glMatIn, mdsMatrix<4, double> & matOut);

	template<class ARG_TYPE> void glMatFromM3AndVec(ARG_TYPE * glMatOut, const mdsMatrix<3, ARG_TYPE> & matIn, const mdsVector<3, ARG_TYPE> & vecIn);

	mdsMatrix<4, double> InvertMdsMat(const mdsMatrix<4, double> & matIn);
};


template<class ARG_TYPE> void matrixGlobals::glMatFromM3AndVec(ARG_TYPE * glMatOut, 
															   const mdsMatrix<3, ARG_TYPE> & matIn, 
															   const mdsVector<3, ARG_TYPE> & vecIn)
{
	for(int nRow = 0; nRow < 3; ++nRow)
	{
		for(int nCol = 0; nCol < 3; ++nCol)
		{
			glMatOut[4*nRow+nCol] = matIn[nRow][nCol];
		}
	}
	for(int nCol = 0; nCol < 3; ++nCol)
	{
		glMatOut[12+nCol]=vecIn[nCol];
	}
	glMatOut[15] = 1.0;
	glMatOut[3] = glMatOut[7] = glMatOut[11] = 0.0;
}

